/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: CoordinateDriveClientExample.cpp,v 1.10 2009-09-25 10:31:18 morsko Exp $
 *
 * \file
 * \brief This is an example for using the CoordinateDrive interface.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 */
#include "CoordinateDriveClient.hpp"
#include "CoordinateDriveData.hpp"
#include <curses.h>
#include <math.h>
//*****************************************************************************
#define GIMI_CLIENT_API 20000
#include "gimi.h"
//*****************************************************************************
// Objects
static bool run = true;
static gimi::GIMI g;
static MaCI::CoordinateDrive::CCoordinateDriveClient cdClient(&g, 0);


//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
static int verbose = 1;
//*****************************************************************************
//*****************************************************************************

static void DoCoordinateDrive()
{
  // Initialize Curses (After this point, don't use printf family calls!)
  initscr(); 
  cbreak(); 
  //noecho();
  nonl();
  intrflush(stdscr, FALSE);
  keypad(stdscr, TRUE);
  scrollok(stdscr, TRUE);
  //nodelay(stdscr, TRUE);
  //halfdelay(10);

  // After this point, we don't want dPrint to print to Screen, so we disable it.
  debugSetPrintLevel(0);
  debugSetLogLevel(5);
  
  clear();
  printw("Welcome to CoordinateDrive test program.\n\n");
 
  int seq = -1;
  int coordSeq = -1;
  int serverStatusSeq =-1;
  do {
    int r;
    int value;
    printw("\nSelect CoordinateDrive Query:\n");
    printw("a = Set new Coordinate\n"
	   "b = Remove coordinates\n"
	   "c = Start\n"
           "d = Stop\n"
	   "e = Continue\n"
	   "f = Set mode\n"
	   "g = Unset mode\n"
	   "h = Wait for event\n"
	   "i = Get Coordinates\n"
	   "j = Get Open drive modes\n"
	   "k = Send 1000 coordinates\n"
	   "l = Get length of the path\n"
	   "m = Get number of points in the path\n"
	   "n = Get single coordinate from the path\n"
           "o = Get plan between 2 points\n"
           "p = Get maximum driving speed\n"
           "r = Set Driving speed\n"
           "s = Get newest Server status event\n"
	   "q = QUIT\n");
    printw("?\n");
    value = getch();
    clear(); // After input, clear the screen. Not after processing!
    switch(value){
    case 'a':{
      float x=0.0,y=0.0,a=0.0;
      int temp;
      bool diff= false;
      bool pathPoint = false;
      bool waitP= false;
      printw("Enter coordinateValues: <x y>");
      r = scanw("%f %f",&x, &y);
      if(r>=2){
	printw("Enter angle:");
	r = scanw("%f",&a);
	if(r>=1){
	  printw("is it a Differential Coordinate (0 no, others for yes):");
	  r = scanw("%d",&temp);
	  if(r>=1){
	    if(temp != 0){
	      diff = true;
	    }
	    printw("is it a Path Coordinate (0 no, others for yes):");
	    r = scanw("%d",&temp);
	    if(r>=1){
	      if(temp != 0){
		pathPoint = true;
	      }
	      printw("is it a wait point (0 no, others for yes):");
	      r = scanw("%d",&temp);
	      if(r>=1){
		if(temp != 0){
		  waitP = true;
		}
                MaCI::CoordinateDrive::TCoordinate2D coord(x,y,a,diff,pathPoint,waitP);
                cdClient.SetCoordinate(coord);
                  /*		cdClient.SetCoordinate(x, 
				       y, 
				       a,
				       diff,
				       pathPoint,
				       waitP);*/
		printw("Sending....");
	      }
	       
	     }
	  }
	}
      }
      break;
    }
    case 'b':{
      printw("Removing coordinates...");
      if(cdClient.RemoveCoordinates()){
	printw("Success\n");
      }else{
	printw("Failed\n");
      }
      break;
    }
    case 'c':{
      printw("Calling for start...");
      if(cdClient.SetStart()){
	printw("Success\n");
      }else{
	printw("Failed\n");
      }
      break;
    }
    case 'd':{
      printw("Calling for Stop...");
      if(cdClient.SetStop()){
	printw("Success\n");
      }else{
	printw("Failed\n");
      }
      break;
    }
    case 'e':{
      printw("Calling for Continue...");
      if(cdClient.Continue()){
	printw("Success\n");
      }else{
	printw("Failed\n");
      }
      break;
    }
    case 'f':{
      int temp;
      printw("choose mode to set\n");
      printw("0 = Use Reverse\n");
      printw("1 = Turn first toward TargetCoordinate\n");
      printw("2 = Turn to wanted angle when arrived to goal\n");
      printw("3 = Path driving:");
      r = scanw("%d",&temp);
      if(r>=1){
	if(temp == 0)	cdClient.SetDriveMode(MaCI::CoordinateDrive::KModeUseReverse);
	else if(temp == 1) cdClient.SetDriveMode(MaCI::CoordinateDrive::KModeTurnFirstTowardGoal);
	else if(temp == 2) cdClient.SetDriveMode(MaCI::CoordinateDrive::KModeUseAngle);
	else if(temp == 3) cdClient.SetDriveMode(MaCI::CoordinateDrive::KModePathDriving);
      }
      break;
    }
    case 'g':{
      int temp;
      printw("choose mode to unset\n");
      printw("0 = Use Reverse\n");
      printw("1 = Turn first toward TargetCoordinate\n");
      printw("2 = Turn to wanted angle when arrived to goal\n");
      printw("3 = Path driving:");
      r = scanw("%d",&temp);
      if(r>=1){
	if(temp == 0)	cdClient.UnsetDriveMode(MaCI::CoordinateDrive::KModeUseReverse);
	else if(temp == 1) cdClient.UnsetDriveMode(MaCI::CoordinateDrive:: KModeTurnFirstTowardGoal);
	else if(temp == 2) cdClient.UnsetDriveMode(MaCI::CoordinateDrive::KModeUseAngle);
	else if(temp == 3) cdClient.UnsetDriveMode(MaCI::CoordinateDrive::KModePathDriving);
      }
      break;
    }
    case 'h':{
      //MaCI::CoordinateDrive::CCoordinateDriveData cData;
      MaCI::CoordinateDrive::TCoordinate2D coord;
      MaCI::Common::TTimestamp tstamp;
      printw("Waiting event for 2 sec...\n");
      if(cdClient.GetLatestPointReached(coord,
                                        tstamp,
					&seq,
					2000)){
	//	const MaCI::CoordinateDrive::TCoordinate2D *coord = cData.GetCoordinate(0);
	printw("GOT IT!\n"
	       "point reached %f %f %f, differential %d, pathTarget %d,waitPoint %d\n",
	       coord.x,
	       coord.y,
	       coord.a,
               coord.isDifferential,
	       coord.isPathTarget,
	       coord.isWaitPoint);
      }else{
	printw("TIMEOUT\n");
      }
      break;
    }

    case 'i':{
      MaCI::CoordinateDrive::TCoordinateArray aArray;
      MaCI::Common::TTimestamp tstamp;
      printw("Getting coordinates(1000 ms timeout)....sequence %d ",coordSeq);
      if(cdClient.GetCoordinates(aArray,tstamp,&coordSeq,1000)){
	printw("OK! Array size %d\n",
	       aArray.size());
	for(unsigned int i = 0; i< aArray.size(); ++i){
	
	  printw("%d. ",i);
	  printw("time %u %u ,(x,y,a)=(%f,%f,%f),Differential %d, pathTarget %d, waitPoint %d\n",
                 tstamp.timestamp_s,
                 tstamp.timestamp_us,
		 aArray[i].x,
		 aArray[i].y,
		 aArray[i].a,
		 aArray[i].isDifferential,
		 aArray[i].isPathTarget,
		 aArray[i].isWaitPoint);
		 

	}
      }else{
	printw("Failed!\n");
      }
      
      break;
    }
    case 'j':{
      MaCI::CoordinateDrive::TDriveModeArray aArray;
      printw("Getting open drive modes(1000 ms timeout)....");
      if(cdClient.GetOpenDrivingModes(aArray,1000)){
	printw("OK! Array.size %d\n",
	       aArray.size());
	for(unsigned int i = 0; i< aArray.size(); ++i){
	  printw("%d. ",i);
	  printw("DriveMode: %d\n",
		 aArray[i].mode);
	}
      }else{
	printw("Failed!\n");
      }
      break;
    }
    case 'k':{
      for(int i = 0; i < 1500; i++){
	cdClient.SetCoordinate((float)i, 
			       (float)i, 
			       0.5,
			       false,
			       false,
			       false);
      }
      printw("Sending....");

      break;
    }
    case 'l':{
      float lin;
      float rot;
      if (cdClient.GetPathLength(&lin, &rot)){
        printw("Path lengths are lin:%f[m], rot:%f[rad] \n", lin, rot);
      }
      else{
        printw("FAILED receiving path....\n");
      }
      break;
    }
    case 'm':{
      /*  int i = -1;
      MaCI::CoordinateDrive::TCoordinateContainer points;
      if (cdClient.GetCoordinates(points,1000)){
        i = int(points.GetArraySize());
      }
      printw("Path has %d points\n", i);
      */
      break;
    }
      
    case 'n':{
      /*      unsigned int i;
      MaCI::CoordinateDrive::TCoordinate2D point;
      MaCI::CoordinateDrive::TCoordinateContainer points;
      printw("choose number of coordinate:");
      r = scanw("%u",&i);
      
      if(cdClient.GetCoordinates(points,1000)){
        if (points.GetArraySize() > i){
          point = points.GetArrayElement(i);
          printw("(x,y,a)=(%f,%f,%f), Differential %d, pathTarget %d, waitPoint %d\n",
                 point.x,
                 point.y,
                 point.a,
                 point.isDifferential,
                 point.isPathTarget,
                 point.isWaitPoint);
          break;
        }
        printw("Index out of range\n");
      }
      printw("Could not get point\n");
      */
      break;
    }

    case 'o':{

      float x=0.0,y=0.0,a=0.0;
    
      bool diff= false;
      bool pathPoint = false;
      bool waitP= false;
      unsigned int timeouttime = 5000;
      MaCI::CoordinateDrive::TCoordinateArray plan;
      printw("Enter coordinateValues for starting coordinate: <x y>");
      r = scanw("%f %f",&x, &y);
      if(r>=2){
	printw("Enter angle:");
	r = scanw("%f",&a);
	if(r>=1){
          const MaCI::CoordinateDrive::TCoordinate2D from(x,y,a,diff,pathPoint,waitP);

          printw("Enter coordinateValues for ending coordinate: <x y>");
          r = scanw("%f %f",&x, &y);
          if(r>=2){
            printw("Enter angle:");
            r = scanw("%f",&a);
            if(r>=1){
              const MaCI::CoordinateDrive::TCoordinate2D to(x,y,a,diff,pathPoint,waitP);
              
              printw("Sending values and waiting for response, timeout time(%u ms)\n",timeouttime);
              if(cdClient.GetPlan(from,
                                  to,
                                  plan,
                                  timeouttime)){
                printw("GOT REPLY! (%u points)\n",plan.size());

                for(unsigned int i = 0; i< plan.size(); i++){

                  printw("%d. (x,y,a)=(%f,%f,%f), Differential %d, pathTarget %d, waitPoint %d\n",
                         i+1,
                         plan[i].x,
                         plan[i].y,
                         plan[i].a,
                         plan[i].isDifferential,
                         plan[i].isPathTarget,
                         plan[i].isWaitPoint);
                  
                }




              }else{
                printw("Failed to get reply!\n");

              }
            }
            
          }
        }
      }

      break;
    }

    case 'p':{
      MaCI::CoordinateDrive::TSpeed speeds;
      if(cdClient.GetMaximumDrivingSpeed(speeds,3000)){

        printw("Got maximum driving speed. speed %f, angularSpeed %f",
               speeds.speed,
               speeds.angularSpeed);
      }else{
        printw("Failed to get max driving speeds");
      }

      break;
    }
    case 'r':{
      float speed, angularSpeed;
      printw("Enter new driving speed: <speed angularSpeed>\n");
      r = scanw("%f %f",&speed, &angularSpeed);
      if(r>=2){
        const MaCI::CoordinateDrive::TSpeed speeds(speed,angularSpeed);
        if(cdClient.SetDrivingSpeed(MaCI::CoordinateDrive::TSpeed(speed,angularSpeed))){
	printw("Success\n");
        }else{
          printw("Failed\n");
        }

      }else{
        printw("Not enough parameter given. Ignoring...");
      }
      break;
    }

    case 's':{
      MaCI::CoordinateDrive::CCoordinateDriveData data;
      MaCI::Common::TTimestamp tstamp;
      unsigned int timeouttime = 3000;
      printw("Getting coordinateDrive server status (timeout %u) (sequence %d)\n",timeouttime,serverStatusSeq);
      
      if(cdClient. GetServerStatusMessage(data,
                                          tstamp,
                                          &serverStatusSeq,
                                          timeouttime)){

        printw("Got server status message (sequence %d)\n",serverStatusSeq);
        const MaCI::CoordinateDrive::TCommand *command = data.GetCommand();
        switch(command->cmd){
        case MaCI::CoordinateDrive::KCommandUnableToDriveToCoordinate:{
          const MaCI::CoordinateDrive::TCoordinate2D *coord = data.GetCoordinate(0);
          
          printw("Got status: Unable to drive to coordinate: (x,y,a)=(%f,%f,%f), Differential %d, pathTarget %d, waitPoint %d\n",
                 coord->x,
                 coord->y,
                 coord->a,
                 coord->isDifferential,
                 coord->isPathTarget,
                 coord->isWaitPoint);

          break;
        }
        case MaCI::CoordinateDrive::KCommandCalculating:{
          printw("Got status: Calculating\n");
          
          
          break;
        }              
        case MaCI::CoordinateDrive::KCommandPathReady:{
          printw("Got status: Path Ready\n");
          
          break;
        }              

        default:{
          printw("Got status: UNIDENTIFIED STATUS\n");
          break;
        }
        }
        
      }else{
        printw("Failed. No new server status within timeout time\n");
      }


      break;
    }
    case 'q':
     
      run = false;
      printw("\nBYE BYE!.\n\n");
      
      break;

    default:
      clear();
      printw("\nUnknown command.\n\n");
      continue;
      break;
    }
    
  } while(run);

  // Close curses.
  endwin();

}
//*****************************************************************************
void showUsage()
{
  fprintf(stderr,"\n");
  fprintf(stderr,"Usage:\tCoordinateDriveClientExample [OPTIONS]\n");
  fprintf(stderr,"  [-u addr]\t\t\tUse 'addr' as GIMnet AccessPoints address\n");
  fprintf(stderr,"  [-p port]\t\t\tUse 'port' as GIMnet AccessPoints port\n");
  fprintf(stderr,"  [-n name]\t\t\tUse 'name' as this nodes GIMnet name\n");
  fprintf(stderr,"  [-s name]\t\t\tUse 'name' as CoordinateDrive target\n");
  fprintf(stderr,"  \t\t\t\tOR\n");
  fprintf(stderr,"  [-S name]\t\t\tSpecify the data source by MaCISL\n\n");
  fprintf(stderr,"  [-v]\t\t\t\tIncrease verbosity by 1\n");
  fprintf(stderr,"  [-q]\t\t\t\tQuiet mode\n");
  fprintf(stderr, "\n\n");
}
//*****************************************************************************

int main(int argc, char *argv[]) {
  // GIMnet parameters
  std::string gimnetAP = "asrobo.hut.fi";
  int gimnetAPPort = 40002;
  std::string gimnetName = "";
  std::string gimnetSourceName = "";
  std::string datasourceMaCISL;
  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(8);
  debugSetLogFilename("CoordinateDrive_Client_example.log");
  debugSetLogLevel(10);
  // Greet
  fprintf(stderr,
	  "\nCoordinateDriveExample - MaCI CoordinateDrive Interface Example - v0.0\n");

  // Parse parameters
  bool parse = true;
  while(parse) {
    int c;
    c = getopt(argc, argv, "S:s:n:u:p:hqv");
    switch(c) {

      /////////////////////////////////
      ////// Application specific /////
      /////////////////////////////////

      /////////////////////////////////
      ///// GIMnet parameters /////////
      /////////////////////////////////
    case 's':
      gimnetSourceName = optarg;
      dPrint(3,"Using GIMnet source name '%s'", optarg);
      break;

    case 'S':
      datasourceMaCISL = optarg;
      dPrint(3,"Using DataSource MaCISL '%s'", optarg);
      break;
    case 'n':
      gimnetName = optarg;
      dPrint(3,"Using GIMnet Name '%s'", optarg);
      break;

    case 'u':
      gimnetAP = optarg;
      dPrint(3,"Using GIMnetAP address '%s'", optarg);
      break;

    case 'p':
      gimnetAPPort = atoi(optarg);
      dPrint(3,"Using GIMnetAP port '%d'", gimnetAPPort);
      break;

      //////////////////////////////////
      //// GENERIC:                 ////
      //////////////////////////////////
    case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
      if (verbose > 0)
	debugSetGlobalDebugLvl(++verbose);
      break;
      
    case 'q':
      verbose = 0;
      debugSetGlobalDebugLvl(0);
      break;
      
    case 'h':
    case '?':
      showUsage();
      exit(1);
      break;
      
    case -1:
      parse = false;
      break;
    }
  }


  MaCI::EMaCIError e;
  
  int r;
  if ( (r = g.connectToHubEx(gimnetAP, gimnetAPPort, gimnetName) ) != GIMI_OK) {
    dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
  } else {
    dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());


    // Set target

    if (gimnetSourceName.size()) {
      cdClient.SetDefaultTarget(gimnetSourceName);
    }else{
       // GIMI is Open(), so we can execute this.
      dPrint(ODINFO,"Attempting to set the target using MaCISL - this may take up to 3000ms to complete.");
      cdClient.SetDefaultTarget(MaCI::MaCICtrl::SMaCISL(datasourceMaCISL), 3000);
      
    }
    
    // Attempt to Open Client
    if ( (e = cdClient.Open()) != MaCI::KMaCIOK) {
      dPrint(1,"Failed to Open CoordinateDriveClient instance! (%s)",
	     GetErrorStr(e).c_str());
      
    } else {

      DoCoordinateDrive();
      cdClient.Close();
    }
  }
  
  return 0;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************










